#include "showApiGnss.h"
#include "utm_transformer.h"


c_showApiGnss::c_showApiGnss(/* args */)
{

    
}

c_showApiGnss::~c_showApiGnss()
{
}


ros::Publisher *p_Gnss_path_pub;
nav_msgs::Path Gnsspath;


void c_showApiGnss::pubMesage_ros(float x, float y)
{
    geometry_msgs::PoseStamped this_pose_stamped;
    this_pose_stamped.pose.position.x = x;
    this_pose_stamped.pose.position.y = y;

    geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(0);
    this_pose_stamped.pose.orientation.x = goal_quat.x;
    this_pose_stamped.pose.orientation.y = goal_quat.y;
    this_pose_stamped.pose.orientation.z = goal_quat.z;
    this_pose_stamped.pose.orientation.w = goal_quat.w;

    this_pose_stamped.header.stamp=ros::Time::now();
    this_pose_stamped.header.frame_id="odom";
    Gnsspath.poses.push_back(this_pose_stamped);

    p_Gnss_path_pub->publish(Gnsspath);
    ros::spinOnce();               // check for incoming messages
}



double first_point_x = 0; // 使用numeric_limits方法将定义的变量设置极值
double first_point_y = 0;
double meter_to_centi=100.0; // 米到厘
double testx = 0;
double testy = 0;
void c_showApiGnss::Callback(const MowerProtoPakg::localizationGNGGA& msg) 
{
    double world_coordinate_latitude = 0;
    double world_coordinate_longitude = 0;
    
    if (first_point_x == 0) { // 设置第一个坐标 Set the first coordinate
        first_point_x = msg.latitude();
        first_point_y = msg.longitude();
    } 
    world_coordinate_latitude = (msg.latitude() - first_point_x) * meter_to_centi;
    world_coordinate_longitude = (msg.longitude() - first_point_y) * meter_to_centi;

    printf("GNSS: %f %f   %f %f\r\n", 
        msg.latitude(), msg.longitude(), 
        world_coordinate_latitude, world_coordinate_longitude);

    pubMesage_ros(world_coordinate_latitude, world_coordinate_longitude);
    // pubMesage_ros(testx++, testy++);
}


void c_showApiGnss::loop(void)
{
    ros::NodeHandle ph;
    ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("gnss",1, true);
    p_Gnss_path_pub = &path_pub;
    Gnsspath.header.stamp = ros::Time::now();
    Gnsspath.header.frame_id="odom";

    // recv protobuf of mower_mv    
    eCAL::protobuf::CSubscriber<MowerProtoPakg::localizationGNGGA> subscriber("GNGGAMessage"); // ! 
    subscriber.AddReceiveCallback(std::bind(Callback, std::placeholders::_2)); 
    while (eCAL::Ok()) {
        sleep(10);
    }
    eCAL::Finalize();// finalize eCAL API
}

void c_showApiGnss::start_thread(void)
{
    thread = std::thread(&c_showApiGnss::loop, this);
}
